function [c,ceq,DC,DCeq] = Qcon(Q,N)
% Generate non-linear equality constraint on the rotation matrix, Q 
% (Q'Q = I), along with analytical gradient of the constraint with respect
% to the vectorised rotation matrix.
% Inputs:
% Q: Vectorised rotation matrix
% N: Number of variables in system.

c = []; % No nonlinear inequality constraint
DC = []; % No associated gradient

Q = reshape(Q,N,N); % Reshape Q to matrix from vector
Qprime = Q';
ceq = Qprime*Q - eye(N); % Equality constraint C(Q) = 0  (Q'Q = I)

% ceq contains NxN (nonlinear) equality constraints in 16 variables.
% Construct a matrix DCeq whose ith column is the partial derivative of
% the ith constraint with respect to vec(Q). The ordering of the 
% constraints is with respect to the vec operator.
DCeq = zeros(N^2);
dQ = zeros(N);
dQprime = dQ;

% Loop over elements of Q (q_ij, i = 1,...,N, j=1,...,N) and compute the
% gradient vector of the constraints.
for jj = 1:N

    for ii = 1:N

        dQ(ii,jj) = 1; % dQ/dq_ij
        dQprime(jj,ii) = 1;
        % d(Q'Q-I)/dq_ij = Q'*(dQ/dq_ij) + (dQ/dq_ij)'*Q
        D = Qprime*dQ + dQprime*Q; % Gradient matrix
        DCeq((jj-1)*N+ii,:) = D(:)'; % Vectorise gradient matrix
        dQ(ii,jj) = 0;
        dQprime(jj,ii) = 0;

    end

end

% There are only N(N+1)/2 independent restrictions, since Q'Q is symmetric.
% This will cause fmincon to throw warning messages due to an
% ill-conditioned matrix. Retaining these constraints may also yield 
% inaccurate solutions. The redundant constraints can be removed using 
% the code below, but this slows computation.
ind = tril(ones(N))==1;
ceq = ceq(ind);
DCeq = DCeq(:,ind(:));

end